package UnitTests;

import static org.junit.Assert.*;

import java.io.IOException;

import lejos.robotics.RegulatedMotor;
import lejos.robotics.navigation.Move;
import lejos.robotics.navigation.Move.MoveType;

import org.junit.*;

import org.junit.Before;
import org.junit.Test;

import Gui.MainGui;
import Gui.Simulator.SimulatorGui;
import Program.LabyrinthParser;
import Robot.RobotControl;
import Robot.SensorColour;

import simulator.robot.*;
import simulator.labyrinth.*;
import simulator.position.*;

public class SimulatorTest {
	private static Simulator simulator = new Simulator();
	private static Labyrinth labyrinth;
	private static RobotControl control = new RobotControl();
	private static SimulatorListener simList = new SimulatorListener(simulator, control);;
	private static MainGui window = new MainGui(control);
	private static SimulatorGui simulatorGui = new SimulatorGui();
	
	@BeforeClass
	public static void setUpBeforeClass() {
		control.setMainGui(window);
		// Default position is (20,20) met angle 0
		if (simulator.getLabyrinth() == null) {
			try {
				labyrinth = LabyrinthParser.parse(ClassLoader.getSystemResource("Maze/maze.txt").getFile());
			} catch (IOException e) {
				e.printStackTrace();
			}
			simulator.setLabyrinth(labyrinth);
		}
		
		control.setSimulator(simulator);
		simulatorGui.setSimulator(simulator);
		
		simulatorGui.setSimulatorListener(simList);
		
	}
	
	@Before
	public void setUp() {
		simulator.reset();
		try {
			Thread.sleep(10); // Geef de simulator tijd om zijn huidige beweging af te ronden
		} catch (InterruptedException e) {
			e.printStackTrace();
		}
		simulator.setTravelSpeed(300);
		simulator.setRotateSpeed(300);
		simulator.setRuis(false);
		simList.setRuis(false);
	}

	@Test
	public void testDrivingAround() {
		// We kunnen niet rechtdoor, want we hebben een labyrinth ingeladen :D
		simulator.rotate(90);
		simulator.travel(40);
		assertEquals(simulator.getPosition(), new Position(20, 60, 90));
	}
	
	@Test
	public void testSpeed() {
		// default
		assertTrue(Double.compare(simulator.getTravelSpeed(), 300) == 0);
		assertTrue(Double.compare(simulator.getRotateSpeed(), 300) == 0);
		
		// Andere waarde
		simulator.setTravelSpeed(50);
		simulator.setRotateSpeed(200);
		assertTrue(Double.compare(simulator.getTravelSpeed(), 50.0) == 0);
		assertTrue(Double.compare(simulator.getRotateSpeed(), 200.0) == 0);
		
		// Max
		simulator.setTravelSpeed(10000);
		simulator.setRotateSpeed(10000);
		assertTrue(Double.compare(simulator.getTravelSpeed(), simulator.getMaxTravelSpeed()) == 0);
		assertTrue(Double.compare(simulator.getRotateSpeed(), simulator.getRotateMaxSpeed()) == 0);
	}
	
	@Test
	public void testDistanceSensor() {
		// We bevinden ons in het midden van het vakje.
		// De afstandssensor heeft op dit moment geen offset.
		assertTrue(simList.getSonarDistance() == 20);
		simulator.getSensorMotor().rotateTo(90);
		assertTrue(simList.getSonarDistance() == 100);
	}
	
	@Test
	public void testLightSensor() {
		simList.start();
		
		// Geef die shit wa tijd om op te starten
		try {
			Thread.sleep(100);
		} catch (InterruptedException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		}
		
		assertEquals(control.getLightColour(), SensorColour.BROWN);
		simulator.rotate(90);
		// De lichtsensor staat 4cm voor het midden
		simulator.travel(16);
		try {
			// De listener updatet maar om de 50ms
			Thread.sleep(100);
		} catch (InterruptedException e) {
			e.printStackTrace();
		}
		assertEquals(control.getLightColour(), SensorColour.WHITE);
	}
	
	@Test
	public void testTouchSensor() {
		assertFalse(simulator.leftIsPressed());
		assertFalse(simulator.rightIsPressed());
		// Bots tegen de muur
		simulator.travel(100);
		assertTrue(simulator.leftIsPressed());
		assertTrue(simulator.rightIsPressed());
		simulator.reset();
		// Bots tegen de muur
		simulator.rotate(-30);
		simulator.travel(100);
		assertFalse(simulator.leftIsPressed());
		assertTrue(simulator.rightIsPressed());
	}
	
	@Test
	public void testOtherThread() throws InterruptedException {
		assertTrue(!simulator.isMoving());
		simulator.rotate(90, true);
		
		Thread.sleep(10); // Give it some time to initialize
		
		assertTrue(simulator.isMoving());
		assertEquals(simulator.getMovement().getMoveType(), MoveType.ROTATE);
		
		Thread.sleep(50);
		
		assertTrue(simulator.isMoving());
		assertEquals(simulator.getMovement().getMoveType(), MoveType.ROTATE);
		
		Thread.sleep(300);
		
		assertFalse(simulator.isMoving());
		assertEquals(simulator.getMovement().getMoveType(), MoveType.STOP);
		
		simulator.travel(40, true);
		
		Thread.sleep(10);
		
		assertTrue(simulator.isMoving());
        assertEquals(simulator.getMovement().getMoveType(), MoveType.TRAVEL);
        
        Thread.sleep(50);
        
        assertTrue(simulator.isMoving());
        assertEquals(simulator.getMovement().getMoveType(), MoveType.TRAVEL);
        
        Thread.sleep(300);

        assertFalse(simulator.isMoving());
        assertEquals(simulator.getMovement().getMoveType(), MoveType.STOP);
	}
	
	@Test
	public void testGetMovement() {
		simulator.rotate(90, true);
		try {
			Thread.sleep(200);
		} catch (InterruptedException e) {
			e.printStackTrace();
		}
		
		Move move = simulator.getMovement();
		
		assertEquals(move.getMoveType(), MoveType.ROTATE);
		assertTrue(move.getAngleTurned() > 0);
		assertTrue(move.getAngleTurned() < 90);
	}
	
	@Test
	public void testSensorMotor() {
	    RegulatedMotor motor = simulator.getSensorMotor();
	    assertTrue(motor.getTachoCount() == 0);
	    motor.rotateTo(90);
	    assertTrue(motor.getTachoCount() == 90);
	    assertTrue(simList.getSonarDistance() == 100);
	    motor.rotateTo(-90);
	    assertTrue(simList.getSonarDistance() == 20);
	}
	
}
